#include "KickToGoalBehaviour.h"


// Zoznam informacii o hracoch, ktorych vidi hrac
extern CEvaluatingPlayerList* ePlayerList;

KickToGoalBehaviour::KickToGoalBehaviour()
{
	lfShootPosX = 52.5;
	lfShootPosY = 0.0;
}

/*
 * Vykopnutie na branku od Loptosov - v jednoduchosti je krasa.
 * x-ova pozicia kam sa striela lopta je vzdy 52.5 a y-ova je bud +/- 6.66
 * podla toho ci y-ova pozicia lopty je +/-
 */
void KickToGoalBehaviour::Behave()
{
	//double PolBrany = world->params->server->goal_width / 2.0;
	lfShootPosX = (double) 52.5;

	//  double ohodH = ePanesMap->CalculateSamplingPassLineEvaluation(world->ball->pos.GetX(), world->ball->pos.GetY(),
	//                                                                lfShootPosX, -6.66, ePanesMap->GetPaneSize()/3.2);
	//  double ohodD = ePanesMap->CalculateSamplingPassLineEvaluation(world->ball->pos.GetX(), world->ball->pos.GetY(),
	//                                                                lfShootPosX, +6.66, ePanesMap->GetPaneSize()/3.2);
	//3.2 je magicke cislo, lebo 3^2 = 9 a 2^3 = 8

	if (world->ball->pos.GetY() < 0) //(ohodH > ohodD)
	lfShootPosY = (double) -6.66;
	else
	lfShootPosY = (double) +6.66;

	/*fprintf(stderr, "[89] u%d t%d: kopem na branu: bod = (%f, %f)\n", world->me->GetUniform(),
		world->state->GetSimTime(), lfShootPosX, lfShootPosY);*/
	GetKickBehaviour()->Behave(Point((float)lfShootPosX, (float)lfShootPosY));
}

/* 
 * Vykopnutie na branku (presnejsie do stredu branky)
 */
void KickToGoalBehaviour::KickToMiddleGoal()
{
	SKILLS_DBGN("%i: ->kicking to goal (2)", world->state->GetSimTime());

	Point target(52.5f, 0);
	GetPassBallToBehaviour()->Behave(target, world->params->server->ball_speed_max);
}

/*
 * Vykopnutie na branku, hlada pripadnych obrancov stojacich v ceste lopty.
 * Ak najde obrancov, hlada po krokoch optimalne miesto pre kopnutie lopty.
 * Ak je brankar mimo, nestriela do stredu ale do stvrtiny brany.
 */
void KickToGoalBehaviour::KickToGoalThruDefence()
{
	TXTN("%i: ->kicking to goal (3)", world->state->GetSimTime());

#define MAX_OBRANCOV 32
#define SEARCH_STEP 0.2f
	// Relative angles to all defenders
	Angle obrancovia[MAX_OBRANCOV];
	int obrancov = 0;
	const float halfgoal = world->params->server->goal_width / 2;
	const float sirka = halfgoal * 0.9f;

	// Default place for kicking
	float posX = 52.5f;
	float posY = 0;

	float pomY;
	int i;
	Point enemy_pos;

	// Najdi vsetkych obrancov, ktori stoja v ceste
	for(i = 0; i < world->enemies.GetSize(); i++)
	{
		enemy_pos = world->enemies[i].pos;

		if((enemy_pos - world->me->pos).GetLength() > 3 && enemy_pos.GetX() > world->me->pos.GetX() &&
			((enemy_pos.GetY() <= world->me->pos.GetY() && enemy_pos.GetY() >= -halfgoal) ||
			(enemy_pos.GetY() >= world->me->pos.GetY() && enemy_pos.GetY() <= halfgoal)
			))
		{
			assert(obrancov < MAX_OBRANCOV);
			DBG("%d [%.2f,%2f] ", world->enemies[i].GetUniform(), enemy_pos.GetX(), enemy_pos.GetY());
			obrancovia[obrancov] = (enemy_pos - world->ball->pos).GetAngle();
			obrancov++;
		}
	}
	DBGN(" are defenders (%d) [%.2f,%.2f]", obrancov, world->me->pos.GetX(), world->me->pos.GetY());

	// Ak sa nasli nejaki obrancovia, tak pohladaj optimalne miesto pre kopnutie
	if(obrancov > 0)
	{
		float max_vaha = -1;

		for(pomY = -sirka; pomY <= sirka; pomY += SEARCH_STEP)
		{
			// Velke cislo
			float vaha = 360;
			Angle moj_uhol = (Point(posX,pomY) - world->ball->pos).GetAngle();

			for(i = 0; i < obrancov; i++)
			{
				const float uhol = (moj_uhol - obrancovia[i]).GetAbsDeg();
				if(uhol < vaha) vaha = uhol;
			}

			if(vaha > max_vaha)
			{
				max_vaha = vaha;
				posY = pomY;
			}
		}
	}

//	PassBallTo(Point(posX, posY), world->params->server->ball_speed_max);
	// ak je brankar mimo, nestrielat do stredu brany, ale do stvrtiny
	if(posY == 0)
	{
//		TXTN("hack");
		posY = halfgoal / 2.f;
		if(world->me->pos.GetY() < 0) posY = -posY;
	}

	if(posY > 0)
		posY -= 0.15f;
	else if(posY < 0)
		posY += 0.15f;

	GetKickBehaviour()->Behave(Point(posX, posY));
#undef SEARCH_STEP
#undef MAX_OBRANCOV
}

/*
 * Kopanie na branu cez metodu vyuzivajucu ohodnocovaciu mapu.
 * Ak nenajde brankara, urci sa bod strelom blizko tyce, ak najde,
 * vypocita tento bod pomocou metody z ohodnocovacej mapy
 */
void KickToGoalBehaviour::KickToGoalEvalPanesMap()
{
	static Player *enemyGoalie;
	static int i;
	
	enemyGoalie = NULL;
	for(i = 0; i < world->enemies.GetSize(); i++)
	{
		if(world->enemies[i].IsGoalie()) 
		{
			enemyGoalie = &(world->enemies[i]);
			break;
		}
	}
	
	lfShootPosX = (double) world->ball->pos.GetX();
	lfShootPosY = (double) world->ball->pos.GetY();

	if(enemyGoalie == NULL)
	{
		Point myPosition = world->me->pos;
		float lfHalfGoalSize = world->params->server->goal_width / 2.0f;
		if((myPosition.GetY() < lfHalfGoalSize + 2.0) && (myPosition.GetY() > 0.5 * lfHalfGoalSize))
		{
			lfShootPosX = 52.5;
			lfShootPosY = 0.8 * lfHalfGoalSize;
		}
		else if((myPosition.GetY() > -lfHalfGoalSize - 2.0) && (myPosition.GetY() < 0.5 * -lfHalfGoalSize))
		{
			lfShootPosX = 52.5;
			lfShootPosY = -0.8 * lfHalfGoalSize;
		}
	}
	else
	{
		ePlayerList->CalculateOptimalShootPointAux(lfShootPosX, lfShootPosY, world->params->server->goal_width / 2.0);
	}
	GetKickBehaviour()->Behave(Point((float)lfShootPosX, (float)lfShootPosY));
}


/*
 * Mal by hrac vystrelit na branku?
 *
 * @return ci ma hrac strielat na branku
 */
bool KickToGoalBehaviour::ShouldKickOnGoal()
{
	if(DATA_STORAGE.DistanceToGoal(world->ball->pos) > 25)
		return false;

	Player *enemyGoalie = DATA_STORAGE.GetEnemyGoalie();
	static int i;

	if(enemyGoalie == NULL)
	{
		// Brankara nevidim, ale moze byt prazdna branka
		if(world->me->IsLookingAt(Point(52.5, 0))) 
			return true;
		else
			return false;
	}

/*	if(enemyGoalie == NULL || enemyGoalie->GetAge() < 5)
	{
//		if(world->me->IsLookingAt(world->params->flags[GRT]) && 
//			world->me->IsLookingAt(world->params->flags[GRB])) return true;
		return false;
	}
*/

	float posX = world->params->flags[GOAL_R].GetX();
	Point tGoal(posX, -(world->params->server->goal_width / 2) * .9f);
	Point bGoal(posX, (world->params->server->goal_width / 2) * .9f);
	Vector tVect = tGoal - world->ball->pos;
	Vector bVect = bGoal - world->ball->pos;

	int maxCycleB, maxCycleT, cycle, cycleB, cycleT;
	float edp, ekp;
	Vector v, a;
	float catchableArea;

	const int MAX_CYCLE = 30;

	Point res, T1, T2;
	float deg;
	int cycleb;
	Point futBposT[MAX_CYCLE];
	Point futBposB[MAX_CYCLE];
	Point futureHePos[MAX_CYCLE];
	float runDist[MAX_CYCLE];
	int maxCycle;
	bool found;

	Player p(*enemyGoalie);
	Ball tmpBall(*world->ball);

	catchableArea = max(world->params->server->catchable_area_l, world->params->server->catchable_area_w);
	catchableArea *= 0.95f;

	// manual page 44
	edp = enemyGoalie->type->effort_max * enemyGoalie->type->dash_power_rate * 100;
	if(edp > world->params->server->player_accel_max) edp = world->params->server->player_accel_max;

	// manual page 48
	ekp = world->params->server->maxpower * world->params->server->kick_power_rate;
	float dir_diff = ((world->ball->pos - world->me->pos) - world->me->GetAbsDir()).GetAngle().GetAbsDeg();
	float dist_ball = (world->ball->pos - world->me->pos).GetLength() - world->params->server->ball_size - world->params->server->player_size;
	ekp *= 1.f - 0.25f * dir_diff / 180.f - 0.25f * dist_ball / world->me->type->kickable_margin;

	// lopta smerujuca k hornej a dolnej tycke
	Ball ballT(*world->ball);
	Ball ballB(*world->ball);
	ballT.vel.SetLength(ekp);
	ballT.vel.SetAngle(tVect.GetAngle());
	ballB.vel.SetLength(ballT.vel.GetLength());
	ballB.vel.SetAngle(bVect.GetAngle());

	deg = fabsf(p.GetAbsDir().GetDeg() - world->ball->vel.GetAngle().GetDeg());
	
	if(deg > 180)
		deg = 360 - deg;
	if(deg > 90)
		deg = 180; // pojde opacne (celom vpred)
	else
		deg = 0;

	futureHePos[0] = p.pos;
	runDist[0] = 0;
	v.SetLength(0);
	maxCycleB = maxCycleT = -1;
	
	for(cycleb = 1; cycleb < MAX_CYCLE; cycleb++)
	{
		// bez otacania
		a = PolarVector(edp, p.GetAbsDir() + DegAngle(deg));
		p.vel += a;
		if(p.vel.GetLength() > p.type->player_speed_max) p.vel.SetLength(p.type->player_speed_max);
		futureHePos[cycleb] = futureHePos[cycleb - 1] + p.vel;
		p.vel *= p.type->player_decay;

		// s otacanim
		if(cycleb > 1) 
		{
			v.SetLength(v.GetLength() + edp);
			
			if(v.GetLength() > p.type->player_speed_max)
				v.SetLength(p.type->player_speed_max);
		}
		runDist[cycleb] = runDist[cycleb - 1] + v.GetLength();
		
		if(cycleb > 1)
			v.SetLength(v.GetLength() * p.type->player_decay);

		// ball
		ballT.Step();
		ballB.Step();
		
		if(fabsf(ballT.pos.GetX()) > 52.5 && maxCycleT == -1)
			maxCycleT = cycleb;
		if(fabsf(ballB.pos.GetX()) > 52.5 && maxCycleB == -1)
			maxCycleB = cycleb;
		if(maxCycleB != -1 && maxCycleT != -1)
			break;
		
		futBposT[cycleb] = ballT.pos;
		futBposB[cycleb] = ballB.pos;
	}
	maxCycle = cycleb;
	
	if(maxCycleT == -1)
		maxCycleT = MAX_CYCLE;
	if(maxCycleB == -1)
		maxCycleB = MAX_CYCLE;

	found = true;

	p = *enemyGoalie;
	// bude sa pohybovat zotrvacnostou zatial co sa otaca
	p.pos = p.pos + p.vel;

	for(cycleB = 0; cycleB < maxCycleB; cycleB++)
	{
		// najskor skus bez otacania
		for(cycle = cycleB; cycle >= 0; cycle--)
		{
			if((futBposB[cycleB + 1] - futureHePos[cycle]).GetLength() <= catchableArea)
			{
				if(fabsf(futureHePos[cycle].GetX()) <= 52.5)
				{
					found = false;
					break;
				}
			}
		}
		if(!found)
			break;

		// teraz skus s otacanim
		for(cycle = cycleB; cycle >= 0; cycle--)
		{
			if((futBposB[cycleB + 1] - p.pos).GetLength() <= runDist[cycle] + catchableArea)
			{
				found = false;
				break;
			}
		}
	}

	if(found) 
	{ 
		TXTN("%d: Quite surely should be goal1", world->state->GetSimTime());
		TXTN(" (%.2f, %.2f, %.1f), (%.2f, %.2f) - %.2f", 
			enemyGoalie->pos.GetX(),
			enemyGoalie->pos.GetY(),
			enemyGoalie->GetAbsDir().GetDeg(),
			world->ball->pos.GetX(),
			world->ball->pos.GetY(),
			ekp);
		return found;
	}

	p = *enemyGoalie;
	// bude sa pohybovat zotrvacnostou zatial co sa otaca
	p.pos = p.pos + p.vel;

	for(cycleT = 0; cycleT < maxCycleT; cycleT++)
	{
		// najskor skus bez otacania
		for(cycle = cycleT; cycle >= 0; cycle--)
		{
			if((futBposT[cycleT + 1] - futureHePos[cycle]).GetLength() <= catchableArea)
			{
				if(fabsf(futureHePos[cycle].GetX()) <= 52.5)
				{
					found = false;
					break;
				}
			}
		}
		if(!found) break;

		// teraz skus s otacanim
		for(cycle = cycleT; cycle >= 0; cycle--)
		{
			if((futBposT[cycleT + 1] - p.pos).GetLength() <= runDist[cycle] + catchableArea)
			{
				found = false;
				break;
			}
		}
	}

	if(found)
	{
		TXTN("%d: Quite surely should be goal2", world->state->GetSimTime());
		TXTN(" (%.2f, %.2f, %.1f), (%.2f, %.2f) - %.2f", 
			enemyGoalie->pos.GetX(),
			enemyGoalie->pos.GetY(),
			enemyGoalie->GetAbsDir().GetDeg(),
			world->ball->pos.GetX(),
			world->ball->pos.GetY(),
			ekp);
	}

	return found;
}